#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     ProtoBoard,     sensorI2CCustomFastSkipStates9V)
#pragma config(Sensor, S4,     IRMux,          sensorI2CCustomFastSkipStates)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop, reversed, encoder)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop, reversed, encoder)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     RightBack,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     LeftBack,      tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S2_C1_1,     RightFront,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     LeftFront,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     Lift1,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     Lift2,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C3_1,     Lift3,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C3_2,     Lift4,         tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    LeftGripper1,         tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    LeftGripper2,         tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    RightGripper1,        tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    RightGripper1,        tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    LeftGripper2,         tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    RightGripper2,        tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    Forward,              tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C2_6,    Side,                 tServoContinuousRotation)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "3785_motion_functions.h"	//Motion fuctions such as forwards and backwards
void initializeRobot()  //Initalizes servos before autonomous starts
{
	return;
}
task main()
{
	initializeRobot();
  waitForStart();
  initalizeEncoders();
  straight(60,8.5);
  stopRobot();
  initalizeEncoders();
  pause();
  turnRight(50,1500);
}
